#!/bin/bash
export ROS_WS_PATH=/opt/ros/noetic
export KEZI_WS_PATH=/home/ukz/process/kit_ws

source $ROS_WS_PATH/setup.bash
source $KEZI_WS_PATH/devel/setup.bash

# 定义要检查的节点名称
NODE1="/joy_node"
NODE2="/kezi_joy_node"

LAUNCH_PKG="kezi_joy"
LAUNCH1="joy.launch"

# 检查节点是否已在运行
check_nodes() {
    if rosnode list | grep -q "$NODE1" || rosnode list | grep -q "$NODE2"; then
        echo "错误：节点 $NODE1 或 $NODE2 已在运行！"
        rosnode list
        exit 1
    fi
}

# 启动launch文件函数
start_launch() {
    echo "启动 launch 文件: $1..."
    roslaunch $1 $2
    if [ $? -ne 0 ]; then
        echo "启动 $1 失败！"
        exit 1
    fi
}

# 主程序
check_nodes

start_launch $LAUNCH_PKG $LAUNCH1
sleep 1  # 给launch一些启动时间

echo "所有节点已启动完成"
